/*
 * Copyright (c) 2014 The Plasix Authors
 * Plasix (Px) Library
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
#ifndef PX_TRIANGLE_H
#define PX_TRIANGLE_H

#include <pxVector3.h>

namespace Px {
namespace Geometry {

/*
 * The triangle class provides an implementation of the abstract geometric
 * primitive based on three individual points (a, b, c). This class provides
 * the functionality for both 2 and 3 dimensional triangles and any integral
 * or floating point numerical types. Note: For integral numerical Real types,
 * some of the operations may become in accurate or invalid under specific
 * circumstances.
 */
template <typename Real>
class Triangle {
    static_assert(std::is_integral<Real>::value || std::is_floating_point<Real>::value, "[Px:Triangle:type] Error: Triangle Real type must be an integral or floating point numerical representation.");

    /* Utilizng a type alias to reference the templated Vector3 class */
    using Vector3 = Math::Vector3<Real>;
    enum Vertex { A, B, C };

public:
    Triangle();
    Triangle(const Triangle<Real>& triangle);
    Triangle(const Vector3& a, const Vector3& b, const Vector3& c);
    virtual ~Triangle();

    /* Functions that set the position of each point (alias of vertex). */
    bool setPoint(unsigned int index, const Vector3& point);
    void setPointA(const Vector3& point);
    void setPointB(const Vector3& point);
    void setPointC(const Vector3& point);

    /* Functions that set the position of each vertex. */
    bool setVertex(unsigned int index, const Vector3& vertex);
    void setVertexA(const Vector3& vertex);
    void setVertexB(const Vector3& vertex);
    void setVertexC(const Vector3& vertex);

    /* Returns the point at the specified index (a = 0, b = 1, c = 2). */
    bool getPoint(unsigned int index, Vector3& point) const;
    bool getVertex(unsigned int index, Vector3& point) const;

    /*
     * This function will determine if the provided point lies within the
     * face of this triangle.
     * 
     * Parameters:
     *   point - The point to be tested.
     *
     * Return: If the point lies within the face of the triangle then this
     * function will return true; otherwise the point lies outside of the
     * triangle and this function will return false.
     */
    bool contains(const Vector3& point, Real epsilon = 1.0e-4) const;

    /*
     * This function returns the Barycentric coordinates for the provided point.
     * 
     * Parameters:
     *   point - The point that will be referenced for calculating the
     *   Barycentric coordinates.
     *   u - Barycentric coordinate component
     *   v - Barycentric coordinate component
     *   w - Barycentric coordinate component
     *   epsilon - Threshold for calculating the Barycentric Coordiantes
     * 
     * Return: If the point is within this triangle then the function will
     * return true; otherwise it will return false;
     */
    bool barycenter(const Vector3& point, Real& u, Real& v, Real& w, Real epsilon = 1.0e-6) const;

    /* Retrieve each of the points/vertices of this triangle. */
    Vector3 getPointA() const;
    Vector3 getPointB() const;
    Vector3 getPointC() const;
    Vector3 getVertexA() const;
    Vector3 getVertexB() const;
    Vector3 getVertexC() const;

    /* Calculates the normal to the face of this triangle. */
    Vector3 normal() const;

    /* Calculates the geometric centroid of this triangle. */
    Vector3 centroid() const;

    /*
     * Calculates the closest point on this triangle to the provided point.
     * There are several forms that this closest point can take:
     *
     *  (1) Closest point is on the face of the triangle
     *  (2) Closest point is equal to one of the three vertices of this triangle
     *  (3) Closest point is on one of this triangles edges
     *
     * Return: Regardless of the case, this function will return the closest 
     * point. This function does not report which primitive the closest point 
     * resides on (see: pointOnEdge(..), pointOnFace(..), pointOnVertex(..))
     */
    Vector3 calculateClosestPoint(const Vector3& point) const;

    /*
     * This function calculates the coefficients of the plane that this triangle
     * resides in (i.e. this triangle is coplanar with the resulting plane).
     */
    void planarCoefficients(double& a, double& b, double& c, double& d) const;

    /* Returns the area of the surface of this triangles face. */
    double area() const;

    /* 
     * Returns the length of one side of this triangle. Indices 0, 1, and 2 are
     * valid and represent each of the three edges.
     */
    double edgeLength(unsigned int index);

    /* 
     * Calculates the perimeter of this triangle as the sum of the magnitudes
     * of the edge vectors of the face.
     */
    double perimeter() const;

    /*
     * This function will calculate the distance between the provided point and
     * the plane that is coplanar with the face of this triangle. The point does
     * not need to reside within the bounds (orthogonally) of this triangle. The
     * function will simply return the distance to the infinite plane that this
     * triangle resides in.
     */
    double pointToPlaneDistance(const Vector3& point) const;

    Vector3 project(const Vector3& vector) const;

    /*
     * If the provided point is on any of the edges of this triangle then the
     * function will return true; otherwise it will return false. The distance
     * threshold for considering the point 'on' the edge is defined by the
     * epsilon value.
     */
    bool pointOnEdge(const Vector3& point, Real epsilon = 1.0e-6) const;

    /*
     * This function will determine if the provided point is on the surface
     * of this triangles face (i.e. anywhere within the triangle). This includes
     * two checks:
     *   (1) The point is within the bounds of the triangle (face)
     *   (2) The point is within the epsilon distance to the plane of the face.
     */
    bool pointOnFace(const Vector3& point, Real epsilon = 1.0e-6) const;
    bool pointOnVertex(const Vector3& point, Real epsilon = 1.0e-6) const;

    /* Constaccessors for each of the three points (a, b, c). */
    Vector3 getA() const;
    Vector3 getB() const;
    Vector3 getC() const;

    /* Non-const reference accessors to the Vector3 points (a, b, c). */
    Vector3& a();
    Vector3& b();
    Vector3& c();

    /* 
     * Triangle Barycentric Coordinates
     * Real-time Collision Detection (Christer Ericson) pg.47, 48.
     * 
     * Parameters:
     *    a - The first point of the triangle (1/3)
     *    b - The second point of the triangle (2/3)
     *    c - The third point of the triangle (3/3)
     *    p - The point that lies within the plane of the triangle but may be
     *         inside the triangle or outside of the triangle
     *    u - (out) The u component of the barycentric coordinate
     *    v - (out) The v component of the barycentric coordinate
     *    w - (out) The w component of the barycentric coordinate
     *    epsilon - Used for 'selection' rather than an exact mathematical 
     *    result. Using epsilon will allow for a threshold around the 
     *    triangle where this function will return true even if the 
     *    point is just 'close' to the triangle.
     * Return: True if the point is within the triangle, otherwise false.
     */
    static bool BarycentricCoordinates(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p, Real& u, Real& v, Real& w, Real epsilon = 1.0e-6);

    /* Calculates the distance between a point and a line segment. */
    static double PointToSegmentDistance(const Vector3& point, const Vector3& segment_p0, const Vector3& segment_p1);

    static bool IntersectSegment(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& segment_p0, const Vector3& segment_p1, Vector3& intersectionPoint);

    /* Returns the surface area of this triangle in units. */
    static double Area(const Vector3& a, const Vector3& b, const Vector3& c);

    /* Returns the perimeter of the edges of this triangle. */
    static double Perimeter(const Vector3& a, const Vector3& b, const Vector3& c);

    /* Project the provided vector onto the plane of the triangle. */
    static Vector3 Project(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& vector);

    /* 
     * Calculates the surface normal for a triangle from the three provided
     * points. The cross-product between two edge vectors is utilized to
     * define the perpendicular surface normal. The resulting vector is
     * normalized.
     * 
     * Parameters:
     *    a - The first point of the triangle
     *    b - The second point of the triangle
     *    c - THe third point of the triangle
     *
     * Return: The normalized surface normal of the triangle defined by the
     * points a, b, and c.
     */
    static Vector3 Normal(const Vector3& a, const Vector3& b, const Vector3& c);

    /*
     * Standard Logical Operators: Since triangles do not lend themselves to
     * an obvious logical quantity, the surface area of the triangle is used.
     */
    bool operator < (const Triangle<Real>& triangle);
    bool operator <= (const Triangle<Real>& triangle);
    bool operator > (const Triangle<Real>& triangle);
    bool operator >= (const Triangle<Real>& triangle);

    Triangle<Real>& operator = (const Triangle<Real>& triangle);

    const static unsigned char TRIANGLE_VERTEX_COUNT = 3;

protected:
    Vector3 vertices[TRIANGLE_VERTEX_COUNT];
};

template <typename Real>
Triangle<Real>::Triangle() {
    this->vertices[A] = Vector3(-1, -1, 0);
    this->vertices[B] = Vector3(0, 1, 0);
    this->vertices[C] = Vector3(1, -1, 0);
}

template <typename Real>
Triangle<Real>::Triangle(const Triangle<Real>& triangle) {
    this->vertices[A] = triangle.vertices[A];
    this->vertices[B] = triangle.vertices[B];
    this->vertices[C] = triangle.vertices[C];
}

template <typename Real>
Triangle<Real>::Triangle(const Vector3& a, const Vector3& b, const Vector3& c) {
    this->vertices[A] = a;
    this->vertices[B] = b;
    this->vertices[C] = c;
}

template <typename Real>
Triangle<Real>::~Triangle() {}

template <typename Real>
bool Triangle<Real>::setPoint(unsigned int index, const Vector3& point) {
    if ( index >= Triangle<Real>::TRIANGLE_VERTEX_COUNT ) {
        std::cerr << "[Px:Triangle:setPoint] Error: Index out of bounds: " << index << std::endl;
        return false;
    }

    this->vertices[index] = point;
    return true;
}

template <typename Real>
void Triangle<Real>::setPointA(const Vector3& point) {
    this->vertices[A] = point;
}

template <typename Real>
void Triangle<Real>::setPointB(const Vector3& point) {
    this->vertices[B] = point;
}

template <typename Real>
void Triangle<Real>::setPointC(const Vector3& point) {
    this->vertices[C] = point;
}

template <typename Real>
bool Triangle<Real>::setVertex(unsigned int index, const Vector3& vertex) {
    if ( index >= Triangle<Real>::TRIANGLE_VERTEX_COUNT ) {
        std::cerr << "[Px:Triangle:setVertex] Error: Index out of bounds: " << index << std::endl;
        return false;
    }

    this->vertices[index] = vertex;
    return true;
}

template <typename Real>
void Triangle<Real>::setVertexA(const Vector3& vertex) {
    this->vertices[A] = vertex;
}

template <typename Real>
void Triangle<Real>::setVertexB(const Vector3& vertex) {
    this->vertices[B] = vertex;
}

template <typename Real>
void Triangle<Real>::setVertexC(const Vector3& vertex) {
    this->vertices[C] = vertex;
}

template <typename Real>
bool Triangle<Real>::getPoint(unsigned int index, Vector3& point) const {
    if ( index >= Triangle<Real>::TRIANGLE_VERTEX_COUNT ) {
        std::cerr << "[Px:Triangle:getPoint] Error: Index out of bounds: " << index << std::endl;
        return false;
    }

    point = this->vertices[index];
    return true;
}

template <typename Real>
bool Triangle<Real>::getVertex(unsigned int index, Vector3& vertex) const {
    if ( index >= Triangle<Real>::TRIANGLE_VERTEX_COUNT ) {
        std::cerr << "[Px:Triangle:getPoint] Error: Index out of bounds: " << index << std::endl;
        return false;
    }

    vertex = this->vertices[index];
    return true;
}

template <typename Real>
bool Triangle<Real>::contains(const Vector3& point, Real epsilon) const {
    Real u, v, w;
    bool inTriangle = Triangle<Real>::BarycentricCoordinates(this->vertices[A], this->vertices[B], this->vertices[C], point, u, v, w);
    Real distance = this->pointToPlaneDistance(point);
    
    if ( (distance < epsilon) && inTriangle ) return true;
    return false;
}

template <typename Real>
bool Triangle<Real>::barycenter(const Vector3& point, Real& u, Real& v, Real& w, Real epsilon) const {
    return Triangle<Real>::BarycentricCoordinates(this->vertices[A], this->vertices[B], this->vertices[C], point, u, v, w, epsilon);
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::getPointA() const {
    return this->vertices[A];
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::getPointB() const {
    return this->vertices[B];
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::getPointC() const {
    return this->vertices[C];
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::getVertexA() const {
    return this->vertices[A];
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::getVertexB() const {
    return this->vertices[B];
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::getVertexC() const {
    return this->vertices[C];
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::normal() const {
    return Triangle<Real>::Normal(this->vertices[A], this->vertices[B], this->vertices[C]);
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::centroid() const {
    double ot = (1.0 / 3.0);
    double cx = (this->vertices[A].getX() + this->vertices[B].getX() + this->vertices[C].getX()) * ot;
	double cy = (this->vertices[A].getY() + this->vertices[B].getY() + this->vertices[C].getY()) * ot;
	double cz = (this->vertices[A].getZ() + this->vertices[B].getZ() + this->vertices[C].getZ()) * ot;
	return Vector3(static_cast<Real>(cx), static_cast<Real>(cy), static_cast<Real>(cz));
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::calculateClosestPoint(const Vector3& point) const {
    Vector3 edge0 = this->vertices[B] - this->vertices[A];
    Vector3 edge1 = this->vertices[C] - this->vertices[A];
    Vector3 v0 = this->vertices[A] - point;

    double a = edge0.dot(edge0);
    double b = edge0.dot(edge1);
    double c = edge1.dot(edge1);
    double d = edge0.dot(v0);
    double e = edge1.dot(v0);

    double det = a*c - b*b;
    double s = b*e - c*d;
    double t = b*d - a*e;

    if ( s + t < det ) {
        if ( s < 0.0 ) {
            if ( t < 0.0 ) {
                if ( d < 0.0 ) {
                    s = Math::Clamp(-d/a, 0.0, 1.0);
                    t = 0.0;
                }
                else {
                    s = 0.0;
                    t = Math::Clamp(-e/c, 0.0, 1.0);
                }
            }
            else {
                s = 0.0;
                t = Math::Clamp(-e/c, 0.0, 1.0);
            }
        }
        else if ( t < 0.0 ) {
            s = Math::Clamp(-d/a, 0.0, 1.0);
            t = 0.0;
        }
        else {
            double invDet = 1.0 / det;
            s *= invDet;
            t *= invDet;
        }
    }
    else {
        if ( s < 0.0 ) {
            double tmp0 = b+d;
            double tmp1 = c+e;
            if ( tmp1 > tmp0 ) {
                double numer = tmp1 - tmp0;
                double denom = a-2*b+c;
                s = Math::Clamp(numer / denom, 0.0, 1.0);
                t = 1-s;
            }
            else {
                t = Math::Clamp(-e/c, 0.0, 1.0);
                s = 0.0;
            }
        }
        else if ( t < 0.0 ) {
            if ( a+d > b+e ) {
                double numer = c + e - b - d;
                double denom = a - 2*b + c;
                s = Math::Clamp(numer/denom, 0.0, 1.0);
                t = 1-s;
            }
            else {
                s = Math::Clamp( -e/c, 0.0, 1.0 );
                t = 0.0;
            }
        }
        else {
            double numer = c + e - b - d;
            double denom = a - 2*b + c;
            s = Math::Clamp(numer / denom, 0.0, 1.0);
            t = 1.0 - s;
        }
    }

    return this->vertices[A] + s * edge0 + t * edge1;
}

template <typename Real>
void Triangle<Real>::planarCoefficients(double& a, double& b, double& c, double& d) const {
    Vector3 normal = this->normal();
    const Vector3& va = this->vertices[A];
    const Vector3& vb = this->vertices[B];
    const Vector3& vc = this->vertices[C];

    a = ((vb.y() - va.y()) * (vc.z() - va.z())) - ((vc.y() - va.y()) * (vb.z() - va.z()));
    b = ((vb.z() - va.z()) * (vc.x() - va.x())) - ((vc.z() - va.z()) * (vb.x() - va.x()));
    c = ((vb.x() - va.x()) * (vc.y() - va.y())) - ((vc.x() - va.x()) * (vb.y() - va.y()));
    d = -(a * va.x() + b * va.y() + c * va.z());
    return true;
}

/* Heron's Formula */
template <typename Real>
double Triangle<Real>::area() const {
    return Triangle<Real>::Area(this->vertices[A], this->vertices[B], this->vertices[C]);
}

template <typename Real>
double Triangle<Real>::edgeLength(unsigned int index) {
    switch ( index ) {
    case 0: return Vector3::Distance(this->vertices[A], this->vertices[B]);
    case 1: return Vector3::Distance(this->vertices[B], this->vertices[C]);
    case 2: return Vector3::Distance(this->vertices[C], this->vertices[A]);
    default:
        std::cerr << "[Px:Triangle:edgeLength] Error: Index out of bounds: " << index << std::endl;
        return 0.0;
    };
}

/* Side-vector magnitude perimeter */
template <typename Real>
double Triangle<Real>::perimeter() const {
    return Triangle<Real>::Perimeter(this->vertices[A], this->vertices[B], this->vertices[C]);
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::project(const Vector3& vector) const {
    return Triangle<Real>::Project(this->getA(), this->getB(), this->getC(), vector);
}

template <typename Real>
double Triangle<Real>::pointToPlaneDistance(const Vector3& point) const {
	Vector3 pointOnPlane = this->centroid();
	Vector3 planeNormal = this->normal();
    Vector3 pplv0 = point - pointOnPlane;
    double sb, sn, sd;
	
	sn = -planeNormal.dot(pplv0);
	sd = planeNormal.dot(planeNormal);
	if ( sd == 0.0 ) return 0.0;
	sb = sn / sd;

	pointOnPlane = point + static_cast<Real>(sb) * planeNormal;
	return Vector3::Distance(point, pointOnPlane);
}

template <typename Real>
bool Triangle<Real>::pointOnEdge(const Vector3& point, Real epsilon) const {
    if ( Triangle<Real>::PointToSegmentDistance(point, this->vertices[A], this->vertices[B]) < epsilon ) return true;
    if ( Triangle<Real>::PointToSegmentDistance(point, this->vertices[B], this->vertices[C]) < epsilon ) return true;
    if ( Triangle<Real>::PointToSegmentDistance(point, this->vertices[C], this->vertices[A]) < epsilon ) return true;
    return false;
}

template <typename Real>
bool Triangle<Real>::pointOnFace(const Vector3& point, Real epsilon) const {
    Real u, v, w;
    if ( this->barycenter(point, u, v, w, epsilon ) == false ) return false;
    if ( this->pointToPlaneDistance(point) > epsilon ) return false;
    return true;
}

template <typename Real>
bool Triangle<Real>::pointOnVertex(const Vector3& point, Real epsilon) const {
    if ( Vector3::Distance(this->vertices[A], point) < epsilon ) return true;
    if ( Vector3::Distance(this->vertices[B], point) < epsilon ) return true;
    if ( Vector3::Distance(this->vertices[C], point) < epsilon ) return true;
    return false;
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::getA() const {
    return this->vertices[A];
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::getB() const {
    return this->vertices[B];
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::getC() const {
    return this->vertices[C];
}

template <typename Real>
typename Triangle<Real>::Vector3& Triangle<Real>::a() {
    return this->vertices[A];
}

template <typename Real>
typename Triangle<Real>::Vector3& Triangle<Real>::b() {
    return this->vertices[B];
}

template <typename Real>
typename Triangle<Real>::Vector3& Triangle<Real>::c() {
    return this->vertices[C];
}

template <typename Real>
bool Triangle<Real>::BarycentricCoordinates(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& point, Real& u, Real& v, Real& w, Real epsilon) {
    Vector3 v0 = b - a;
    Vector3 v1 = c - a;
    Vector3 v2 = point - a;
	
	double d00 = v0.dot(v0);
	double d01 = v0.dot(v1);
	double d11 = v1.dot(v1);
	double d20 = v2.dot(v0);
	double d21 = v2.dot(v1);
	double denom = d00 * d11 - d01 * d01;

	//--------------------------------------------------------------------------
	// Calculation of each component. Resolution is limited to the Real
    // representation of this instantiation.
	//--------------------------------------------------------------------------
	v = static_cast<Real>((d11 * d20 - d01 * d21) / denom);
	w = static_cast<Real>((d00 * d21 - d01 * d20) / denom);
	u = static_cast<Real>(1.0 - v - w);

	//--------------------------------------------------------------------------
	// "In general, a point with barycentric coordinates (u, v, w) is inside
	// (or on) the triangle if and only if 0 <= u, v, w, <= 1". Additional 
	// epsilon value is provided to allow for 'close' selections.
	//--------------------------------------------------------------------------
	if ( u < (0.0 - epsilon) || u >= (1.0 + epsilon) ) return false;
	if ( v < (0.0 - epsilon) || v >= (1.0 + epsilon) ) return false;
	if ( w < (0.0 - epsilon) || w >= (1.0 + epsilon) ) return false;

	//--------------------------------------------------------------------------
	// One of the barycentric coordinates (u, v, w) is negative or greater
	// than 1.0, therefore the point is outside of the triangle.
	//--------------------------------------------------------------------------
	return true;
}

template <typename Real>
double Triangle<Real>::PointToSegmentDistance(const Vector3& point, const Vector3& segment_p0, const Vector3& segment_p1) {
	Vector3 v = segment_p1 - segment_p0;
	Vector3 w = point - segment_p0;

	double c1 = w.dot(v);
	if ( c1 <= 0.0 ) return Vector3::Distance(point, segment_p0);

	double c2 = v.dot(v);
	if ( c2 <= c1 ) return Vector3::Distance(point, segment_p1);

	double b = c1 / c2;
	Vector3 Pb = segment_p0 + static_cast<Real>(b) * v;
	return Vector3::Distance(point, Pb);
}

template <typename Real>
bool Triangle<Real>::IntersectSegment(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& segment_p0, const Vector3& segment_p1, Vector3& intersectionPoint) {
    Vector3 u, v, n;
	Vector3 dir, w0, w;
	double r, ad, bd;

	u = b - a;
	v = c - a;
	n = u.cross(v);

	if ( n == Vector3::Zero() )
		return false;

	dir = segment_p1 - segment_p0;
	w0 = segment_p0 - a;
	ad = -n.dot(w0);
	bd = n.dot(dir);
	if ( std::abs(bd) < std::numeric_limits<double>::epsilon() ) {
		if ( ad == 0.0 ) {
			intersectionPoint = Vector3::Zero(); // The segment lies within the plane of the triangle
            // http://geomalgorithms.com/a06-_intersect-2.html
			return true;
		}
		else return false;
	}

	r = ad / bd;
	if ( r < 0.0 || r > 1.0 ) return false;

	Vector3 temp = dir * r;
	Vector3 intersection = segment_p0 + temp;
	intersectionPoint = intersection;

	double uu, uv, vv, wu, wv, D;
	uu = u.dot(u);
	uv = u.dot(v);
	vv = v.dot(v);
	w = intersection - a;
	wu = w.dot(u);
	wv = w.dot(v);
	D = uv * uv - uu * vv;

	long double s, t;
	s = (uv * wv - vv * wu) / D;
	if ( s < 0.0f || s > 1.0f ) return false;
	t = (uv * wu - uu * wv) / D;
	if ( t < 0.0f || (s + t) > 1.0f ) return false;
	return true;
}

template <typename Real>
double Triangle<Real>::Area(const Vector3& a, const Vector3& b, const Vector3& c) {
    //--------------------------------------------------------------------------
	// a, b, and c are the lengths of the sides of this triangle. These lengths
	// are obtained by calculating the magnitude of the triangle side vectors.
	//--------------------------------------------------------------------------
	Vector3 ab = (b - a);
	Vector3 ac = (c - a);
	Vector3 bc = (c - b);

	//--------------------------------------------------------------------------
	// Triangle side lengths.
	//--------------------------------------------------------------------------
	double aLen = ab.norm();
	double bLen = ac.norm();
	double cLen = bc.norm();

	//--------------------------------------------------------------------------
	// p is half the perimeter: p = (a + b + c) / 2.0
	//--------------------------------------------------------------------------
	double p = (aLen + bLen + cLen) * 0.5;

	//--------------------------------------------------------------------------
	// area = sqrt( p * (p - a)(p - b)(p - c) )
	//--------------------------------------------------------------------------
	return std::sqrt(p * (p-aLen) * (p-bLen) * (p-cLen));
}

template <typename Real>
double Triangle<Real>::Perimeter(const Vector3& a, const Vector3& b, const Vector3& c) {
	double aLen = (b - a).norm();
	double bLen = (c - a).norm();
	double cLen = (c - b).norm();
	return aLen + b + c;
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::Project(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& vector) {
    Vector3 normal = Triangle<Real>::Normal(a, b, c);
    Vector3 projected = Vector3::Project(vector, normal);
    return (vector - projected);
}

template <typename Real>
typename Triangle<Real>::Vector3 Triangle<Real>::Normal(const Vector3& a, const Vector3& b, const Vector3& c) {
    Vector3 ab = (b - a);
	Vector3 ac = (c - a);
	return ab.cross(ac).normalized();
}

template <typename Real>
bool Triangle<Real>::operator < (const Triangle<Real>& triangle) {
    if ( this->area() < triangle.area() ) return true;
    return false;
}

template <typename Real>
bool Triangle<Real>::operator <= (const Triangle<Real>& triangle) {
    if ( this->area() <= triangle.area() ) return true;
    return false;
}

template <typename Real>
bool Triangle<Real>::operator > (const Triangle<Real>& triangle) {
    if ( this->area() > triangle.area() ) return true;
    return false;
}

template <typename Real>
bool Triangle<Real>::operator >= (const Triangle<Real>& triangle) {
    if ( this->area() >= triangle.area() ) return true;
    return false;
}

template <typename Real>
Triangle<Real>& Triangle<Real>::operator = (const Triangle<Real>& triangle) {
    if ( this == &triangle ) return *this;

    this->vertices[A] = triangle.vertices[A];
    this->vertices[B] = triangle.vertices[B];
    this->vertices[C] = triangle.vertices[C];
    return *this;
}

typedef Triangle<char> Triangleb;
typedef Triangle<short> Triangles;
typedef Triangle<int> Trianglei;
typedef Triangle<float> Trianglef;
typedef Triangle<double> Triangled;

}

}

#endif
